#include "CollisionSolver.h"






sf::Vector2f CollisionSolver::getTileCenter(int x, int y)
{
	float xf=(m_twidth*x*2)+m_twidth;
	float yf=(m_theight*y*2)+m_theight;

	return sf::Vector2f(xf,yf);

}

bool CollisionSolver::TileEdge(int x, int y, int side)
{
	if (m_grid[x][y]==TT_plat && side==0)
	{
		return true;
	}
	else if (m_grid[x][y]==TT_solid)
	{
		switch (side)
		{
			//straight up y-
			case 2:
				if (y>0)
				{
					if (m_grid[x][y-1]!=TT_solid)
					{
						return true;
					}
				}
				break;
			//right x+
			case 1:
				if (x<m_width-1)
				{
					if (m_grid[x+1][y]!=TT_solid)
					{
						return true;
					}
				}		
				break;
			//straight down y+
			case 0:
				if (y<m_height-1)
				{
					if (m_grid[x][y+1]!=TT_solid)
					{
						return true;
					}
				}
				break;

			//left x-
			case 3:
				if (x>0)
				{
					if (m_grid[x-1][y]!=TT_solid)
					{
						return true;
					}
				}
				break;
		}
	}
	return false;

}

TILETYPE CollisionSolver::getTileType(int x, int y)
{
	if (x<0||x>=m_width)
	{
		return TT_solid;
	}
	if (y<0||y>=m_height)
	{
		return TT_solid;
	}
	return m_grid[x][y];

}

TILETYPE CollisionSolver::getTileType(sf::Vector2f pos)
{
	return getTileType(pos.x/(m_twidth*2),pos.y/(m_theight*2));
}

int CollisionSolver::getSide(sf::Vector2f direction)
{
	if (direction.x==0)
	{
		if (direction.y<0)
		{
			return 0;
		}
		return 2;
	}
	else
	{
		if (direction.x<0)
		{
			return 1;
		}
		return 3;
	}

}


CollisionSolver::CollisionSolver(TILETYPE **grid, int width, int height, int twidth, int theight)
{

	m_grid=grid;
	m_width=width;
	m_height=height;
	m_twidth=twidth;
	m_theight=theight;
	m_sprites=NULL;
	m_numsprites=0;
}

CollisionSolver::~CollisionSolver()
{
    //dtor
	for (int i=0;i<m_width;i++)
	{
		delete []m_grid[i];
	}
	delete []m_grid;
	if (m_sprites!=NULL)
	{
		for (int i=0;i<m_numsprites;i++)
		{
			delete m_sprites[i];
		}
		delete [] m_sprites;
	}
}

float CollisionSolver::DotProjection(sf::Vector2f vector, sf::Vector2f onto)
{
	float length=sqrt((abs(onto.x)*abs(onto.x))+(abs(onto.y)*abs(onto.y)));
	onto.x=onto.x/length;
	onto.y=onto.y/length;

	float project=(onto.x*vector.x)+(onto.y*vector.y);

	return project;
}

bool  CollisionSolver::getCollision(sf::Vector2f *position, sf::Vector2f *velocity,float dt, sf::Vector2f size)
{
	//find ABB for movement zone
	int x0, y0, x1, y1;
	x0=position->x; y0=position->y;
	x1=position->x+(velocity->x*dt); y1=position->y+(velocity->y*dt);
	if (x0>x1){std::swap(x0,x1);}
	if (y0>y1){std::swap(y0,y1);}
	sf::Rect <int> rect=sf::Rect <int>(x0,y0,x1-x0,y1-y0);
	x0-=size.x/2; y0-=size.y/2;
	x1+=size.x/2; y1+=size.y/2;

	//convert to grid coordinates
	x0=x0/(m_twidth*2); x1=x1/(m_twidth*2);
	y0=y0/(m_theight*2); y1=y1/(m_theight*2);

	//move through grid coordinates in zone
	for (int i=x0;i<=x1;i++)
	{
		for (int j=y0;j<=y1;j++)
		{
			TILETYPE tt=getTileType(i,j);
			if ( tt==TT_solid || tt==TT_plat)
			{
				sf::Vector2f tilecenter=getTileCenter(i,j);
				sf::Vector2f relativepos=tilecenter-*position;
				sf::Vector2f majoraxis;
				//check major axis between tilecenter and collider position
				if (std::abs(relativepos.x)>std::abs(relativepos.y))
				{
					majoraxis=sf::Vector2f(relativepos.x,0);
				}
				else
				{
					majoraxis=sf::Vector2f(0,relativepos.y);
				}
				float contactdistance;
				if (majoraxis.x!=0)
				{
					contactdistance=std::abs(majoraxis.x)-(m_twidth)-(size.x/2);
				}
				else
				{
					contactdistance=std::abs(majoraxis.y)-(m_theight)-(size.y/2);
				}
				//project velocity onto normalized major axis using dot product
				float projection=DotProjection((*velocity*dt),majoraxis);
				if (projection>contactdistance && projection>0)
				{
					if (tt==TT_solid)
					{
						if (TileEdge(i,j,getSide(majoraxis)))
						{
							float ratio=contactdistance/projection;

							sf::Vector2f shift;
							shift.x=velocity->x*ratio*dt;
							shift.y=velocity->y*ratio*dt;

							position->x+=shift.x;
							position->y+=shift.y;

							if (majoraxis.x==0)
							{
								if (majoraxis.y>=0)
								{
								velocity->y=0;
								return true;
								}
								velocity->y=0;
							}
							else
							{
								velocity->x=0;
							}
						}
						
					}
					else if (tt==TT_plat)
					{
						//test that we're approaching the tile from above, platforms can only collide from above
						if (majoraxis.x==0 && majoraxis.y>0)
						{
							//test that we're not 'below' the tile by making sure the contact distance is positive, this allows us to fall through easily
							if (contactdistance>=0)
							{
								float ratio=contactdistance/projection;

								velocity->x=velocity->x*ratio;
								velocity->y=velocity->y*ratio;

								position->x+=velocity->x;
								position->y+=velocity->y;
								if (velocity->y>=0)
								{
								velocity->y=0;
								return true;
								}
								velocity->y=0;
							}
						}
					}
				}
			}
		}
	}
	bool b=getPlatCollision(position,velocity,size,rect,dt);

	return b;
}

bool CollisionSolver::getPlatCollision(sf::Vector2f *position, sf::Vector2f *velocity, sf::Vector2f size, sf::Rect <int> rect,float dt)
{
	if (m_numsprites>0)
	{
		for (int i=0;i<m_numsprites;i++)
		{
			//check bounds to make sure we only check platforms in the bounds
			sf::Rect <int> irect=sf::Rect <int>(rect.left-(m_sprites[i]->getTexture()->getSize().x/2),rect.top-(m_sprites[i]->getTexture()->getSize().y/2),
				rect.width+(m_sprites[i]->getTexture()->getSize().x),rect.height+(m_sprites[i]->getTexture()->getSize().x));
			if (m_sprites[i]->getPosition().x>irect.left && m_sprites[i]->getPosition().x<irect.width+irect.left && m_sprites[i]->getPosition().y>irect.top && m_sprites[i]->getPosition().y<irect.top+irect.height)
			{
 				sf::Vector2f relativepos=m_sprites[i]->getPosition()-*position;
				sf::Vector2f majoraxis;
				if (std::abs(relativepos.x)<std::abs(relativepos.y))
				{
					majoraxis=sf::Vector2f(0,relativepos.y);
					float contactdistance;
					contactdistance=std::abs(majoraxis.y)-(m_sprites[i]->getTexture()->getSize().y/2)-(size.y/2);

					//project velocity onto normalized major axis using dot product
					float projection=DotProjection((*velocity*dt),majoraxis);
					if (projection>=contactdistance && contactdistance>0 && projection>0 && majoraxis.y>0)
					{
						float ratio=contactdistance/projection;

						velocity->x=velocity->x*ratio;
						velocity->y=velocity->y*ratio;

						position->x+=velocity->x;
						position->y+=velocity->y;
						if (velocity->y>=0)
						{
							velocity->y=0;
							return true;
						}
						velocity->y=0;


					}
				}

			}
		}
	}
	return false;
}

void CollisionSolver::setSprites(sf::Sprite **sprites, int numsprites)
{
	m_sprites=sprites;
	m_numsprites=numsprites;
}


int CollisionSolver::getNumsprites()
{
	return m_numsprites;
}
sf::Sprite **CollisionSolver::getSprites()
{
	return m_sprites;
}

void CollisionSolver::Update(float dt)
{
	if (m_numsprites>0)
	{
		for (int i=0;i<m_numsprites;i++)
		{
			WorldElement *element=(WorldElement*)m_sprites[i];
			element->Update(dt);
		}
	}
}

WorldElement *CollisionSolver::getElement(sf::Vector2f pos)
{
	if (m_numsprites>0)
	{
		for (int i=0;i<m_numsprites;i++)
		{
			//check bounds
			sf::Rect <int> rect=sf::Rect <int>(m_sprites[i]->getPosition().x-(m_sprites[i]->getTexture()->getSize().x/2)
				,m_sprites[i]->getPosition().y-(m_sprites[i]->getTexture()->getSize().y/2),
				m_sprites[i]->getTexture()->getSize().x,m_sprites[i]->getTexture()->getSize().y);
			if (pos.x>rect.left && pos.x<rect.left+rect.width && pos.y>rect.top && pos.y<rect.top+rect.height)
			{
				WorldElement *element=(WorldElement*)m_sprites[i];
				return element;
			}
		}
	}
	return NULL;
}

void CollisionSolver::Link(std::vector <Actor*> *actors)
{
	p_actors=actors;
}

Actor *CollisionSolver::getActorCollision(Actor *me, sf::Vector2f *velocity, float dt)
{
	for (int i=0;i<p_actors->size();i++)
	{
		if (p_actors->operator[](i)!=me && p_actors->operator[](i)->getCollision()!=CE_skip)
		{
			sf::Vector2f p=me->getPosition();
			sf::Vector2f t=p_actors->operator[](i)->getPosition();
			if (p.y>t.y-32 && p.y<t.y+32)
			{
				if (p.x>t.x-32 && p.x<t.x+32)
				{
					if (p_actors->operator[](i)->getCollision()==CE_collide)
					{
						;
						if (p.x>t.x)
						{
							p_actors->operator[](i)->setPosition(p.x+8,p.y);
							velocity->x+=4;
							
						}
						else
						{
							p_actors->operator[](i)->setPosition(p.x-8,p.y);
							velocity->x-=4;
							
						}
					}
					return p_actors->operator[](i);
				}
			}
		}
	}
	return NULL;
}

std::vector<Actor*> *CollisionSolver::getActorvector()
{
	return p_actors;
}

Actor *CollisionSolver::getActorCollision(Actor *me, sf::Rect <int> rectangle, ACTORTYPE at)
{
	for (int i=0;i<p_actors->size();i++)
	{
		if (p_actors->operator[](i)!=me && p_actors->operator[](i)->getCollision()!=CE_skip && at==p_actors->operator[](i)->getType())
		{
			Actor *a=p_actors->operator[](i);
			sf::Vector2f p=a->getPosition();
			if (p.x>rectangle.left && p.y>rectangle.top)
			{
				if (p.x<rectangle.left+rectangle.width && p.y<rectangle.top+rectangle.height)
				{
					
					return a;
				}
			}
		}
	}
	return NULL;
}